/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os.h"
#include "can.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "oled.h"
#include "oled_data.h"
#include "Caculate.h"
#include "DJI.h"
#include "STP23.h"
#include "wtr_can.h"
#include "delay_us.h"
#include "motor_mg513.h"
#include "math.h"
#include "delay_us.h"
#include "datou.h"
#include "Control.h"
extern char UART4_RX_BUF[UART4_RX_BUF_SIZE];
extern char UART5_RX_BUF[UART5_RX_BUF_SIZE];
extern uint16_t distance,distance_y;
extern uint8_t jiaqu[2];
extern CAN_HandleTypeDef hcan1;
void x_position_control(float ref, DJI_t * motor)
{
	motor->posPID.ref=ref;
	motor->posPID.cur_error=ref-distance;
	if(motor->posPID.cur_error<7&&motor->posPID.cur_error>-7)
	{
		motor->speedPID.output=0;
	}
	else
	{
		if(motor->posPID.cur_error<100&&motor->posPID.cur_error>-100)
		{
			if(motor->posPID.cur_error>0) speedServo(-1400,motor);
			else speedServo(1400,motor);
		}	
		else 
		{
			if(motor->posPID.cur_error>0) speedServo(-3800,motor);
			else speedServo(3800,motor);
		}
	}
}
void y_position_control(float ref, DJI_t * motor)
{
	motor->posPID.ref = ref;
	motor->posPID.fdb = distance_y;
	PID_Calc(&motor->posPID);

	motor->speedPID.ref = motor->posPID.output;
	motor->speedPID.fdb = motor->FdbData.rpm;
	PID_Calc(&(motor->speedPID));
	if(motor->posPID.cur_error<200&&motor->posPID.cur_error>-200)
	{

		if(motor->posPID.cur_error>0) motor->posPID.output=250;
		else motor->posPID.output=-250;
	}	
	if (motor->posPID.cur_error<10&&motor->posPID.cur_error>-10)
	{
		motor->speedPID.ref=0;
		motor->speedPID.fdb=0;
		motor->speedPID.cur_error=0;
		motor->speedPID.error[0]=0;
		motor->speedPID.error[1]=0;
		motor->posPID.cur_error=0;
		motor->posPID.error[0]=0;
		motor->posPID.error[1]=0;
		motor->speedPID.output=0;		
	}
}
void theta_position_control(float ref, DJI_t * motor)
{
	motor->posPID.ref = ref;
	motor->posPID.fdb = motor->AxisData.AxisAngle_inDegree;
	PID_Calc(&motor->posPID);
	
	motor->speedPID.ref = motor->posPID.output;
	motor->speedPID.fdb = motor->FdbData.rpm;
	PID_Calc(&motor->speedPID);

    if(motor->posPID.cur_error<4&&motor->posPID.cur_error>-4)
    {
		motor->speedPID.ref=0;
		motor->speedPID.fdb=0;
		motor->speedPID.cur_error=0;
		motor->speedPID.error[0]=0;
		motor->speedPID.error[1]=0;
		motor->posPID.cur_error=0;
		motor->posPID.error[0]=0;
		motor->posPID.error[1]=0;
		motor->speedPID.output=0;	
    }
}
void z_position_control(float ref, DJI_t * motor)
{
	motor->posPID.ref = ref;
	motor->posPID.fdb = motor->AxisData.AxisAngle_inDegree;
	PID_Calc(&motor->posPID);
	
	motor->speedPID.ref = motor->posPID.output;
	motor->speedPID.fdb = motor->FdbData.rpm;
	PID_Calc(&motor->speedPID);	
    if(motor->posPID.cur_error<1&&motor->posPID.cur_error>-1)
    {
        motor->speedPID.output = -1000;
    }
}
void xyztheta_control(float ref_X,float ref_Y,float ref_z,float ref_theta,DJI_t *motor_x, DJI_t *motor_y,DJI_t *motor_z, DJI_t *motor_theta, CAN_HandleTypeDef *can_handle)
{
    uint8_t flag_total = 0;
	while (!(flag_total)) {
        x_position_control(ref_X,&hDJI[0]);
        y_position_control(ref_Y,&hDJI[1]);
        z_position_control(ref_z,&hDJI[3]);
        theta_position_control(ref_theta,&hDJI[4]);
        CanTransmit_DJI_1234(&hcan1,hDJI[0].speedPID.output, 
          hDJI[1].speedPID.output,
           -hDJI[1].speedPID.output,
            hDJI[3].speedPID.output); 
        CanTransmit_DJI_5678(&hcan1, 
          hDJI[4].speedPID.output,0,0,0);
        osDelay(1);  // 延时1ms        
        // 检查是否达到目标位置并稳定
        if (motor_x->posPID.cur_error < 5 
            && motor_x->posPID.cur_error > -5
            &&motor_y->posPID.cur_error < 10 
            && motor_y->posPID.cur_error > -10
            &&motor_z->posPID.cur_error<3
            &&motor_z->posPID.cur_error>-3
            &&motor_theta->posPID.cur_error<4
            &&motor_theta->posPID.cur_error>-4) {
            CanTransmit_DJI_1234(can_handle, 0, 0, 0, -1000);
            CanTransmit_DJI_5678(can_handle, 0, 0, 0, 0);			
			flag_total = 1;  // 设置完成标志
            osDelay(1);
        }
    }	
}
void claw1heshang(void)
{
    uint16_t i = 0;
    while (i < 2500)
    {
        HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 0); 
        HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 1); 
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 998);	
        CanTransmit_DJI_1234(&hcan1, 0, 0, 0, -1000);        
        osDelay(1);
        i++;
    }
    // 循环结束后复位状态
    HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 0); 
    HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 0); 	
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	jiaqu[0]=1;
}
void claw1songkai(void)
{
    uint16_t i = 0;
    while (i < 2500)
    { 
        HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 1); 
        HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 0); 
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 998);	
        CanTransmit_DJI_1234(&hcan1, 0, 0, 0, -1000);         
        osDelay(1);
        i++;
    }
    // 循环结束后复位状态
    HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 0); 
    HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 0); 	
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	jiaqu[0]=0;	
}
void claw2heshang(void)
{
    uint16_t i = 0;
    while (i < 2500)
    {
        HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 0); 
        HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 1); 
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 998);	
        CanTransmit_DJI_1234(&hcan1, 0, 0, 0, -1000);         
        osDelay(1);
        i++;
    }
    // 循环结束后复位状态
    HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 0); 
    HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 0); 	
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	jiaqu[1]=1;	
}
void claw2songkai(void)
{
    uint16_t i = 0;
    while (i < 2500)
    { 
        HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 1); 
        HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 0); 
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 998);	
        CanTransmit_DJI_1234(&hcan1, 0, 0, 0, -1000);         
        osDelay(1);
        i++;
    }
    // 循环结束后复位状态
    HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 0); 
    HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 0); 	
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	jiaqu[1]=0;	
}
void claw_store_control(u8 storenum,u8 clawnum)
{
    if(clawnum==1)
    {
        switch(storenum)
        {
            case 1:
            xyztheta_control(X1_store,Y2_store,Z3_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X1_store,Y1_store,Z3_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw1heshang();   
            xyztheta_control(X1_store,Y1_store,Z4_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X1_store,Y2_store,Z4_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
                break;
            case 2:
            xyztheta_control(X2_store,Y2_store,Z3_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X2_store,Y1_store,Z3_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw1heshang();   
            xyztheta_control(X2_store,Y1_store,Z4_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X2_store,Y2_store,Z4_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
                break;
            case 3:
            xyztheta_control(X3_store,Y2_store,Z3_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X3_store,Y1_store,Z3_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw1heshang();   
            xyztheta_control(X3_store,Y1_store,Z4_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X3_store,Y2_store,Z4_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);              
                break;
            case 4:
            xyztheta_control(X1_store,Y2_store,Z1_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X1_store,Y1_store,Z1_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw1heshang();   
            xyztheta_control(X1_store,Y1_store,Z2_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X1_store,Y2_store,Z2_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
                break;
            case 5:
            xyztheta_control(X2_store,Y2_store,Z1_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X2_store,Y1_store,Z1_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw1heshang();   
            xyztheta_control(X2_store,Y1_store,Z2_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X2_store,Y2_store,Z2_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
                break;
            case 6:
            xyztheta_control(X3_store,Y2_store,Z1_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X3_store,Y1_store,Z1_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw1heshang();   
            xyztheta_control(X3_store,Y1_store,Z2_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X3_store,Y2_store,Z2_store,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);              
                break;
            default:
                break;
        }
    }
    if(clawnum==2)
    {
        switch(storenum)
        {
            case 1:
            xyztheta_control(X1_store,Y2_store,Z3_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X1_store,Y1_store,Z3_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw2heshang();   
            xyztheta_control(X1_store,Y1_store,Z4_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X1_store,Y2_store,Z4_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
                break;
            case 2:
            xyztheta_control(X2_store,Y2_store,Z3_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X2_store,Y1_store,Z3_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw2heshang();   
            xyztheta_control(X2_store,Y1_store,Z4_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X2_store,Y2_store,Z4_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
                break;
            case 3:
            xyztheta_control(X3_store,Y2_store,Z3_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X3_store,Y1_store,Z3_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw2heshang();   
            xyztheta_control(X3_store,Y1_store,Z4_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X3_store,Y2_store,Z4_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);              
                break;
            case 4:
            xyztheta_control(X1_store,Y2_store,Z1_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X1_store,Y1_store,Z1_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw2heshang();   
            xyztheta_control(X1_store,Y1_store,Z2_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X1_store,Y2_store,Z2_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
                break;
            case 5:
            xyztheta_control(X2_store,Y2_store,Z1_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X2_store,Y1_store,Z1_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw2heshang();   
            xyztheta_control(X2_store,Y1_store,Z2_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X2_store,Y2_store,Z2_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
                break;
            case 6:
            xyztheta_control(X3_store,Y2_store,Z1_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            xyztheta_control(X3_store,Y1_store,Z1_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);     
            claw2heshang();   
            xyztheta_control(X3_store,Y1_store,Z2_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X3_store,Y2_store,Z2_store,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);              
                break;
            default:
                break;
        }
    }    
}
void claw_place_control(u8 storenum,u8 clawnum)
{
    if(clawnum==1)
    {
        switch(storenum)
        {
            case 1:
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_3,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_2,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_3,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);              
            break;
            case 2:
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_2,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);            
            break;
            case 3:
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_2,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                   
            break;
            case 4:
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_2,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);              
            break;
            case 5:
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_2,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);               
            break;
            case 6:
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_3,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_2,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_3,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);               
            break;
            default:
                break;
        }
    }
    if(clawnum==2)
    {
        switch(storenum)
        {
            case 1:
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_3,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_2,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw2songkai();
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_3,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                   
            break;
            case 2:
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_3,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_2,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw2songkai();
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_3,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);            
            break;
            case 3:
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_3,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_2,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw2songkai();
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_3,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                   
            break;
            case 4:
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_3,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_2,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw2songkai();
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_3,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);              
            break;
            case 5:
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_3,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_2,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw2songkai();
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_3,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);               
            break;
            case 6:
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_3,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_2,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw2songkai();
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_3,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);             
            break;
            default:
                break;
        }
    }
}
void claw_stacked_control(u8 storenum)
{
        switch(storenum)
        {
            case 1:
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_3,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_2,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_5,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_5,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);
            xyztheta_control(X_placement_a,Y_placement_a,z_placement_4,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            claw2songkai();
            xyztheta_control(X4_placement_centre,Y3_placement_centre,z_placement_4,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                                                              
            break;
            case 2:
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_2,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_5,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_5,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);
            xyztheta_control(X_placement_b,Y_placement_b,z_placement_4,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            claw2songkai();
            xyztheta_control(X4_placement_centre,Y3_placement_centre,z_placement_4,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                          
            break;
            case 3:
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_2,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_5,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_5,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);
            xyztheta_control(X_placement_c,Y_placement_c,z_placement_4,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            claw2songkai();
            xyztheta_control(X4_placement_centre,Y3_placement_centre,z_placement_4,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);        
            break;
            case 4:
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_2,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_5,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_5,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);
            xyztheta_control(X_placement_d,Y_placement_d,z_placement_4,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            claw2songkai();
            xyztheta_control(X4_placement_centre,Y3_placement_centre,z_placement_4,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                 
            break;
            case 5:
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_3,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_2,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_5,540,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_5,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);
            xyztheta_control(X_placement_e,Y_placement_e,z_placement_4,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            claw2songkai();
            xyztheta_control(X4_placement_centre,Y3_placement_centre,z_placement_4,0,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                           
            break;
            case 6:
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_3,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);                      
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_2,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);    
            claw1songkai();
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_5,810,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);  
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_5,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);
            xyztheta_control(X_placement_f,Y_placement_f,z_placement_4,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1); 
            claw2songkai();
            xyztheta_control(X4_placement_centre,Y3_placement_centre,z_placement_4,270,
            &hDJI[0],&hDJI[1],&hDJI[3],&hDJI[4],&hcan1);               
            break;
            default:
                break;
        }
}
